#define F_CPU 16000000UL

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <stdio.h>
#include <string.h>

#include "led_gen.h"
#include "serial_teste.h"

#define CHANNEL_NB 8
#define SERIAL_BAUD 57600

#define AILERON 0
#define PROFUNDOR 1
#define MOTOR 2
#define LEME 3
#define MODO 4

unsigned int serinp[CHANNEL_NB]; //servo positions

static inline void main_init( void );

ISR(TIMER1_CAPT_vect){

  static unsigned int lasticr; //icr at last caputre
  static unsigned char cserinp; //current input servo
  
  unsigned int licr;

  licr=ICR1-lasticr;
  lasticr=ICR1;
  
  if(licr>4500){ //pulse too long, means start of new frame
    cserinp=0;
  }else if(licr>1000){ //pulse good, take reading, go to next channel
    serinp[cserinp]=licr;
    if(cserinp<CHANNEL_NB){
      cserinp++;
    }
  }else{
    //too short, nothing to see here
  }
}

void setup_timer1(){

  //disable all interupts
  TIMSK1 &= ~( _BV(TOIE1) | _BV(ICIE1) | _BV(OCIE1A) | _BV(OCIE1B));
  
  //set timer mode
  TCCR1A &= ~( _BV(WGM11) | _BV(WGM10) );
  TCCR1B &= ~( _BV(WGM12) | _BV(WGM13) | _BV(ICNC1));
  
  //capture raising edge
  TCCR1B |= _BV(ICES1);
  
  //prescaler 1/8
  TCCR1B |= _BV(CS11);
  TCCR1B &= ~( _BV(CS12) | _BV(CS10) );
  
  //disable outputs
  TCCR1A &= ~( _BV(COM1A0) | _BV(COM1A1) | _BV(COM1B0) | _BV(COM1B1));
  
  //enable capture interupt
  TIMSK1 |= (1<<ICIE1);

  //set ICP1 as input
  DDRB &= ~(1<<DDB0);
  PORTB |= (1<<PORTB0);
  MCUCR &= ~(1<<PUD);
}

static inline void main_init( void ) {

  LED1_INIT;
  setup_timer1();
  serial_init((F_CPU/SERIAL_BAUD/16)-1);

}

char* sprint_channels(char* buf, int* ch_vec, int len) {

  char cur_ch[6];
  int i;

  buf[0] = '\0';
  for (i=0;i<len-1;i++) {
    sprintf(cur_ch,"%d,",serinp[ch_vec[i]]);
    strncat(buf,cur_ch,7);
  }
  sprintf(cur_ch,"%d\n",serinp[ch_vec[i]]);
  strncat(buf,cur_ch,7);

  return buf;

}

int main (void)
{

  int ch_vec [] = {MOTOR,MODO};
  char buf[60];
  int len;

  main_init();
  sei();
  
  len = sizeof(ch_vec)/sizeof(int);

  while (1) {
    LED1_OFF;
    _delay_ms(500);
    LED1_ON;
    sprint_channels(buf,ch_vec,len);
    serial_transmit(buf);
  }
  return 0;
}

